#include "SurfRansacMatcherI.h"
#include <vector>

using namespace std;

SurfRansacMatcherI::SurfRansacMatcherI()
{
	name = "SurfRansacMatcherI";
	printf("new SurfRansacMatcherI\n");
	nr_iter = 500;
	distance_threshold = 0.001f;
}

SurfRansacMatcherI::~SurfRansacMatcherI()
{
	printf("delete SurfRansacMatcherI\n");
}

void SurfRansacMatcherI::update(){
}

const bool debugg_SurfRansacMatcherI = false;
Transformation * SurfRansacMatcherI::getTransformation(FrameNode * src, FrameNode * dst)
{
	if(debugg_SurfRansacMatcherI){printf("SurfRansacMatcherI::getTransformation(%i,%i)\n",src->frame->id_number,dst->frame->id_number);}

	IplImage* img_combine;
	int sx;
	int sy;
	if(debugg_SurfRansacMatcherI)
	{
		IplImage* img_a = cvLoadImage( src->frame->rgb_path.c_str(), 1);
		IplImage* img_b = cvLoadImage( dst->frame->rgb_path.c_str(), 1);
	
		sx = img_a->width;
		sy = img_a->height;
		
		img_combine = cvCreateImage(cvSize(2*sx,sy), IPL_DEPTH_8U, 3);
		for (int i = 0; i < sx; i++)
		{
			for (int j = 0; j < sy; j++)
			{
				img_combine->imageData[3 * (j * (2*sx) + (sx+i)) + 0] = img_a->imageData[3 * (j * sx + i) + 0];
				img_combine->imageData[3 * (j * (2*sx) + (sx+i)) + 1] = img_a->imageData[3 * (j * sx + i) + 1];
				img_combine->imageData[3 * (j * (2*sx) + (sx+i)) + 2] = img_a->imageData[3 * (j * sx + i) + 2];
			}
		}
	
		for (int i = 0; i < sx; i++)
		{
			for (int j = 0; j < sy; j++)
			{
				img_combine->imageData[3 * (j * (2*sx) + (i)) + 0] = img_b->imageData[3 * (j * sx + i) + 0];
				img_combine->imageData[3 * (j * (2*sx) + (i)) + 1] = img_b->imageData[3 * (j * sx + i) + 1];
				img_combine->imageData[3 * (j * (2*sx) + (i)) + 2] = img_b->imageData[3 * (j * sx + i) + 2];
			}
		}
	}
	
	Transformation * transformation = new Transformation();
	transformation->src = src;
	transformation->dst = dst;
	transformation->weight = 100;
	transformation->level = 1;
	
	KeyPointSet * src_keypointset 	= src->frame->keypoints;
	KeyPointSet * dst_keypointset 	= dst->frame->keypoints;
	KeyPoint ** src_keypoints	 	= src->frame->keypoints->valid_key_points;
	KeyPoint ** dst_keypoints 		= dst->frame->keypoints->valid_key_points;

	int src_nr_points = src_keypointset->nr_valid_points;
	int dst_nr_points = dst_keypointset->nr_valid_points;

	float ** surf_distances = new float*[src_nr_points];
	for(int i = 0; i < src_nr_points;i++)
	{
		surf_distances[i] = new float[dst_nr_points];		
		for(int j = 0; j < dst_nr_points;j++)
		{
			FeatureDescriptor * descriptorA = src_keypoints[i]->surf_descriptor;
			FeatureDescriptor * descriptorB = dst_keypoints[j]->surf_descriptor;
			surf_distances[i][j] = descriptorA->distance(descriptorB);
		}
	}
	
	float * pos_src_x = new float[src_nr_points];
	float * pos_src_y = new float[src_nr_points];
	float * pos_src_z = new float[src_nr_points];
	
	float * pos_dst_x = new float[dst_nr_points];
	float * pos_dst_y = new float[dst_nr_points];
	float * pos_dst_z = new float[dst_nr_points];
	
	Eigen::Matrix4f transformationMat = Eigen::Matrix4f::Identity();
	
	for(int i = 0; i < src_nr_points;i++)
	{
		pos_src_x[i] = src_keypoints[i]->position(0);
		pos_src_y[i] = src_keypoints[i]->position(1);
		pos_src_z[i] = src_keypoints[i]->position(2);
		if(debugg_SurfRansacMatcherI){cvCircle(img_combine,cvPoint(src_keypoints[i]->frame_x, src_keypoints[i]->frame_y), 5,cvScalar(0, 0, 255, 0),2, 8, 0);}
	}
	for(int i = 0; i < dst_nr_points;i++)
	{
		pos_dst_x[i] = dst_keypoints[i]->position(0);
		pos_dst_y[i] = dst_keypoints[i]->position(1);
		pos_dst_z[i] = dst_keypoints[i]->position(2);
		if(debugg_SurfRansacMatcherI){cvCircle(img_combine,cvPoint(dst_keypoints[i]->frame_x + sx, dst_keypoints[i]->frame_y), 5,cvScalar(0, 0, 255, 0),2, 8, 0);}
	}
	
	float surf_theshold = 0.2;
	int * src_matches 				= new int[src_nr_points];
	int * dst_matches 				= new int[dst_nr_points];
	for(int i = 0; i < src_nr_points;i++)
	{
		src_matches[i] = -1;
		float best_value = 9999999;
		for(int j = 0; j < dst_nr_points;j++)
		{
			if(surf_distances[i][j]<best_value)
			{
				best_value = surf_distances[i][j];
				src_matches[i] = j;
			}
		}
	}
	for(int j = 0; j < dst_nr_points;j++)
	{
		dst_matches[j] = -1;
		float best_value = 9999999;
		for(int i = 0; i < src_nr_points;i++)
		{
			if(surf_distances[i][j]<best_value)
			{
				best_value = surf_distances[i][j];
				dst_matches[j] = i;
			}
		}
	}

	pcl::TransformationFromCorrespondences tfc;	
	KeyPoint ** src_match_id = new KeyPoint*[src_nr_points];
	KeyPoint ** dst_match_id = new KeyPoint*[src_nr_points];
		
	int * src_matches_id = new int[src_nr_points];
	int * dst_matches_id = new int[src_nr_points];
	int nr_matches = 0;
	
	for(int i = 0; i < src_nr_points;i++)
	{
		if(i == dst_matches[src_matches[i]] && surf_distances[i][src_matches[i]] < surf_theshold){
			src_match_id[nr_matches] = src->frame->keypoints->valid_key_points[i];
			dst_match_id[nr_matches] = dst->frame->keypoints->valid_key_points[src_matches[i]]; 
			nr_matches++;
		}
	}
	if(nr_matches >= 3)
	{
		KeyPoint * src_point;
		KeyPoint * dst_point;
		int best_fit 		= 0;
		int* best_indexes	= new int[nr_matches];
		for(int i = 0; i < nr_iter; i++)
		{
			int match_a = rand() % nr_matches;
			int match_b = rand() % nr_matches;
			int match_c = rand() % nr_matches;
			while(match_b == match_a){match_b = rand() % nr_matches;}
			while(match_c == match_b || match_c == match_a){match_c = rand() % nr_matches;}
			
			tfc.reset();
			tfc.add(src_match_id[match_a]->position, dst_match_id[match_a]->position);
			tfc.add(src_match_id[match_b]->position, dst_match_id[match_b]->position);
			tfc.add(src_match_id[match_c]->position, dst_match_id[match_c]->position);
			
			transformationMat = tfc.getTransformation().matrix();
			float mat00 = transformationMat(0,0);
			float mat01 = transformationMat(0,1);
			float mat02 = transformationMat(0,2);
			float mat03 = transformationMat(0,3);
			float mat10 = transformationMat(1,0);
			float mat11 = transformationMat(1,1);
			float mat12 = transformationMat(1,2);
			float mat13 = transformationMat(1,3);
			float mat20 = transformationMat(2,0);
			float mat21 = transformationMat(2,1);
			float mat22 = transformationMat(2,2);
			float mat23 = transformationMat(2,3);
			float mat30 = transformationMat(3,0);
			float mat31 = transformationMat(3,1);
			float mat32 = transformationMat(3,2);
			float mat33 = transformationMat(3,3);
			int nr_good_fitts = 0;

			for(int j = 0; j < nr_matches; j++)
			{
				src_point = src_match_id[j];
				dst_point = dst_match_id[j];
			
				float x = src_point->px;
				float y = src_point->py;
				float z = src_point->pz;
				
				float dx = x*mat00+y*mat01+z*mat02+mat03-dst_point->px;
				float dy = x*mat10+y*mat11+z*mat12+mat13-dst_point->py;
				float dz = x*mat20+y*mat21+z*mat22+mat23-dst_point->pz;

				if(dx*dx + dy*dy + dz*dz < distance_threshold){nr_good_fitts++;}
			}
			if(best_fit < nr_good_fitts)
			{
				best_fit = nr_good_fitts;
				nr_good_fitts = 0;
				for(int j = 0; j < nr_matches; j++)
				{
					src_point = src_match_id[j];
					dst_point = dst_match_id[j];
					
					float x = src_point->px;
					float y = src_point->py;
					float z = src_point->pz;
				
					float dx = x*mat00+y*mat01+z*mat02+mat03-dst_point->px;
					float dy = x*mat10+y*mat11+z*mat12+mat13-dst_point->py;
					float dz = x*mat20+y*mat21+z*mat22+mat23-dst_point->pz;
					if(dx*dx + dy*dy + dz*dz<distance_threshold){
						best_indexes[nr_good_fitts] = j;
						nr_good_fitts++;
					}
				}
			}
		}
		tfc.reset();
		transformation->matches_src = new KeyPoint*[best_fit];
		transformation->matches_dst = new KeyPoint*[best_fit];
		transformation->nr_matches 	= best_fit;
		for(int i = 0; i < best_fit; i++)
		{
			dst_point = dst_match_id[best_indexes[i]];
			src_point = src_match_id[best_indexes[i]];
			transformation->matches_src[i] = src_point;
			transformation->matches_dst[i] = dst_point;
			tfc.add(src_point->position,dst_point->position);
			if(debugg_SurfRansacMatcherI){
				//printf("match surf distance: %f\n",dst_point->surf_descriptor->distance(src_point->surf_descriptor));
				cvLine(img_combine,cvPoint(src_point->frame_x, src_point->frame_y), cvPoint(dst_point->frame_x+sx, dst_point->frame_y), cvScalar(0, 255, 0, 0), 1, 8, 0);
			}
		}
		transformation->transformationMatrix = tfc.getTransformation().matrix();
		if(debugg_SurfRansacMatcherI){printf("best_fit: %i\n",best_fit);}
		transformation->weight = best_fit*30;
		printf("weight: %f\n",transformation->weight);
	}else{
		transformation->transformationMatrix = Eigen::Matrix4f::Identity();
	}
	if(debugg_SurfRansacMatcherI){cvShowImage("combined image", img_combine);}
	transformation->next = 0;
	transformation->prev = 0;

	return transformation;
}
